<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN"
                "http://www.w3.org/TR/REC-html40/loose.dtd">
<html>
<head>
  <title>Description of cdkf</title>
  <meta name="keywords" content="cdkf">
  <meta name="description" content="CDKF  Central Difference Kalman Filter  (Sigma-Point Kalman Filter variant)">
  <meta http-equiv="Content-Type" content="text/html; charset=iso-8859-1">
  <meta name="generator" content="m2html &copy; 2003 Guillaume Flandin">
  <meta name="robots" content="index, follow">
  <link type="text/css" rel="stylesheet" href="../../m2html.css">
</head>
<body>
<a name="_top"></a>
<div><a href="../../menu.html">Home</a> &gt;  <a href="#">ReBEL-0.2.7</a> &gt; <a href="#">core</a> &gt; cdkf.m</div>

<!--<table width="100%"><tr><td align="left"><a href="../../menu.html"><img alt="<" border="0" src="../../left.png">&nbsp;Master index</a></td>
<td align="right"><a href="menu.html">Index for .\ReBEL-0.2.7\core&nbsp;<img alt=">" border="0" src="../../right.png"></a></td></tr></table>-->

<h1>cdkf
</h1>

<h2><a name="_name"></a>PURPOSE <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="box"><strong>CDKF  Central Difference Kalman Filter  (Sigma-Point Kalman Filter variant)</strong></div>

<h2><a name="_synopsis"></a>SYNOPSIS <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="box"><strong>function [xh, Px, pNoise, oNoise, InternalVariablesDS] = cdkf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS) </strong></div>

<h2><a name="_description"></a>DESCRIPTION <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="fragment"><pre class="comment"> CDKF  Central Difference Kalman Filter  (Sigma-Point Kalman Filter variant)

   [xh, Px, pNoise, oNoise, InternalVariablesDS] = cdkf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)

   This filter assumes the following standard state-space model:

     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]
     y(k) = hfun[x(k),n(k),U2(k)]

   where x is the system state, v the process noise, n the observation noise, U1 the exogenous input to the state
   transition function, U2 the exogenous input to the state observation function and y the noisy observation of the
   system.

   INPUT
         state                state mean at time k-1          ( xh(k-1) )
         Pstate               state covariance at time k-1    ( Px(k-1) )
         pNoise               process noise data structure     (must be of type 'gaussian' or 'combo-gaussian')
         oNoise               observation noise data structure (must be of type 'gaussian' or 'combo-gaussian')
         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )
         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )
         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )
         InferenceDS          inference data structure generated by GENINFDS function.

   OUTPUT
         xh                   estimates of state starting at time k ( E[x(t)|y(1),y(2),...,y(t)] for t=k,k+1,...,k+N-1 )
         Px                   state covariance
         pNoise               process noise data structure     (possibly updated)
         oNoise               observation noise data structure (possibly updated)

         InternalVariablesDS  (optional) internal variables data structure
           .xh_                  predicted state mean ( E[x(t)|y(1),y(2),..y(t-1)] for t=k,k+1,...,k+N-1 )
           .Px_                  predicted state covariance
           .yh_                  predicted observation ( E[y(k)|Y(k-1)] )
           .inov                 innovation signal
           .Pinov                innovation covariance
           .KG                   Kalman gain

   Required InferenceDS fields:
         .spkfParams           SPKF parameters = [h] with
                                    h  :  CDKF scale factor / difference step size

   Copyright (c) Oregon Health &amp; Science University (2006)

   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for
   academic use only (see included license file) and can be obtained from
   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the
   software should contact rebel@csee.ogi.edu for commercial licensing information.

   See LICENSE (which should be part of the main toolkit distribution) for more
   detail.</pre></div>

<!-- crossreference -->
<h2><a name="_cross"></a>CROSS-REFERENCE INFORMATION <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
This function calls:
<ul style="list-style-image:url(../../matlabicon.gif)">
<li><a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>	ADDANGLE   Addition function for 'angle space' sigma-points expressed in radians.</li><li><a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>	CVECREP  Column vector replicate</li><li><a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>	ADDANGLE   Addition function for 'angle space' sigma-points expressed in radians.</li></ul>
This function is called by:
<ul style="list-style-image:url(../../matlabicon.gif)">
<li><a href="../.././ReBEL-0.2.7/examples/parameter_estimation/dempe1.html" class="code" title="">dempe1</a>	DEMPE1  Demonstrate parameter estimation on a simple 2nd order LTI system.</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse1.html" class="code" title="">demse1</a>	DEMSE1  Demonstrate state estimation on a simple 2nd order LTI system.</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse2.html" class="code" title="">demse2</a>	DEMSE2  Demonstrate state estimation on a simple scalar nonlinear (time variant) problem</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse3.html" class="code" title="">demse3</a>	DEMSE3  Demonstrate nonlinear time series state estimation for Mackey-Glass chaotic time series</li></ul>
<!-- crossreference -->


<h2><a name="_source"></a>SOURCE CODE <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="fragment"><pre>0001 <a name="_sub0" href="#_subfunctions" class="code">function [xh, Px, pNoise, oNoise, InternalVariablesDS] = cdkf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)</a>
0002 
0003 <span class="comment">% CDKF  Central Difference Kalman Filter  (Sigma-Point Kalman Filter variant)</span>
0004 <span class="comment">%</span>
0005 <span class="comment">%   [xh, Px, pNoise, oNoise, InternalVariablesDS] = cdkf(state, Pstate, pNoise, oNoise, obs, U1, U2, InferenceDS)</span>
0006 <span class="comment">%</span>
0007 <span class="comment">%   This filter assumes the following standard state-space model:</span>
0008 <span class="comment">%</span>
0009 <span class="comment">%     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]</span>
0010 <span class="comment">%     y(k) = hfun[x(k),n(k),U2(k)]</span>
0011 <span class="comment">%</span>
0012 <span class="comment">%   where x is the system state, v the process noise, n the observation noise, U1 the exogenous input to the state</span>
0013 <span class="comment">%   transition function, U2 the exogenous input to the state observation function and y the noisy observation of the</span>
0014 <span class="comment">%   system.</span>
0015 <span class="comment">%</span>
0016 <span class="comment">%   INPUT</span>
0017 <span class="comment">%         state                state mean at time k-1          ( xh(k-1) )</span>
0018 <span class="comment">%         Pstate               state covariance at time k-1    ( Px(k-1) )</span>
0019 <span class="comment">%         pNoise               process noise data structure     (must be of type 'gaussian' or 'combo-gaussian')</span>
0020 <span class="comment">%         oNoise               observation noise data structure (must be of type 'gaussian' or 'combo-gaussian')</span>
0021 <span class="comment">%         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )</span>
0022 <span class="comment">%         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )</span>
0023 <span class="comment">%         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )</span>
0024 <span class="comment">%         InferenceDS          inference data structure generated by GENINFDS function.</span>
0025 <span class="comment">%</span>
0026 <span class="comment">%   OUTPUT</span>
0027 <span class="comment">%         xh                   estimates of state starting at time k ( E[x(t)|y(1),y(2),...,y(t)] for t=k,k+1,...,k+N-1 )</span>
0028 <span class="comment">%         Px                   state covariance</span>
0029 <span class="comment">%         pNoise               process noise data structure     (possibly updated)</span>
0030 <span class="comment">%         oNoise               observation noise data structure (possibly updated)</span>
0031 <span class="comment">%</span>
0032 <span class="comment">%         InternalVariablesDS  (optional) internal variables data structure</span>
0033 <span class="comment">%           .xh_                  predicted state mean ( E[x(t)|y(1),y(2),..y(t-1)] for t=k,k+1,...,k+N-1 )</span>
0034 <span class="comment">%           .Px_                  predicted state covariance</span>
0035 <span class="comment">%           .yh_                  predicted observation ( E[y(k)|Y(k-1)] )</span>
0036 <span class="comment">%           .inov                 innovation signal</span>
0037 <span class="comment">%           .Pinov                innovation covariance</span>
0038 <span class="comment">%           .KG                   Kalman gain</span>
0039 <span class="comment">%</span>
0040 <span class="comment">%   Required InferenceDS fields:</span>
0041 <span class="comment">%         .spkfParams           SPKF parameters = [h] with</span>
0042 <span class="comment">%                                    h  :  CDKF scale factor / difference step size</span>
0043 <span class="comment">%</span>
0044 <span class="comment">%   Copyright (c) Oregon Health &amp; Science University (2006)</span>
0045 <span class="comment">%</span>
0046 <span class="comment">%   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for</span>
0047 <span class="comment">%   academic use only (see included license file) and can be obtained from</span>
0048 <span class="comment">%   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the</span>
0049 <span class="comment">%   software should contact rebel@csee.ogi.edu for commercial licensing information.</span>
0050 <span class="comment">%</span>
0051 <span class="comment">%   See LICENSE (which should be part of the main toolkit distribution) for more</span>
0052 <span class="comment">%   detail.</span>
0053 
0054 <span class="comment">%=================================================================================================================</span>
0055 
0056 
0057 Xdim  = InferenceDS.statedim;                                <span class="comment">% extract state dimension</span>
0058 Odim  = InferenceDS.obsdim;                                  <span class="comment">% extract observation dimension</span>
0059 U1dim = InferenceDS.U1dim;                                   <span class="comment">% extract exogenous input 1 dimension</span>
0060 U2dim = InferenceDS.U2dim;                                   <span class="comment">% extract exogenous input 2 dimension</span>
0061 Vdim  = InferenceDS.Vdim;                                    <span class="comment">% extract process noise dimension</span>
0062 Ndim  = InferenceDS.Ndim;                                    <span class="comment">% extract observation noise dimension</span>
0063 
0064 NOV = size(obs,2);                                           <span class="comment">% number of input vectors</span>
0065 
0066 <span class="comment">%------------------------------------------------------------------------------------------------------------------</span>
0067 <span class="comment">%-- ERROR CHECKING</span>
0068 
0069 <span class="keyword">if</span> (nargin ~= 8) error(<span class="string">' [ cdkf ] Not enough input arguments.'</span>); <span class="keyword">end</span>
0070 
0071 <span class="keyword">if</span> (Xdim~=size(state,1)) error(<span class="string">' [ cdkf ] Prior state dimension differs from InferenceDS.statedim'</span>); <span class="keyword">end</span>
0072 <span class="keyword">if</span> (Xdim~=size(Pstate,1)) error(<span class="string">' [ cdkf ] Prior state covariance dimension differs from InferenceDS.statedim'</span>); <span class="keyword">end</span>
0073 <span class="keyword">if</span> (Odim~=size(obs,1)) error(<span class="string">' [ cdkf ] Observation dimension differs from InferenceDS.obsdim'</span>); <span class="keyword">end</span>
0074 <span class="keyword">if</span> (U1dim~=0),
0075     [dim,nop] = size(U1);
0076     <span class="keyword">if</span> (U1dim~=dim) error(<span class="string">' [ cdkf ] Exogenous input U1 dimension differs from InferenceDS.U1dim'</span>); <span class="keyword">end</span>
0077     <span class="keyword">if</span> (dim &amp; (NOV~=nop)) error(<span class="string">' [ cdkf ] Number of observation vectors and number of exogenous input U1 vectors do not agree!'</span>); <span class="keyword">end</span>
0078 <span class="keyword">end</span>
0079 <span class="keyword">if</span> (U2dim~=0),
0080     [dim,nop] = size(U2);
0081     <span class="keyword">if</span> (U2dim~=dim) error(<span class="string">' [ cdkf ] Exogenous input U2 dimension differs from InferenceDS.U2dim'</span>); <span class="keyword">end</span>
0082     <span class="keyword">if</span> (dim &amp; (NOV~=nop)) error(<span class="string">' [ cdkf ] Number of observation vectors and number of exogenous input U2 vectors do not agree!'</span>); <span class="keyword">end</span>
0083 <span class="keyword">end</span>
0084 
0085 <span class="comment">%------------------------------------------------------------------------------------------------------------------</span>
0086 
0087 xh   = zeros(Xdim,NOV);
0088 xh_  = zeros(Xdim,NOV);
0089 yh_  = zeros(Odim,NOV);
0090 inov = zeros(Odim,NOV);
0091 
0092 <span class="comment">%------------------------------------------------------------------------------------------------------------------</span>
0093 
0094 h = InferenceDS.spkfParams;
0095 hh = h^2;
0096 
0097 W1 = [(hh - Xdim - Vdim)/hh   1/(2*hh);                  <span class="comment">% sigma-point weights set 1</span>
0098       1/(2*h)                sqrt(hh-1)/(2*hh)];
0099 
0100 W2      = W1;
0101 W2(1,1) = (hh - Xdim - Ndim)/hh ;                        <span class="comment">% sigma-point weights set 2</span>
0102 
0103 Zeros_Xdim_X_Vdim = zeros(Xdim,Vdim);
0104 Zeros_Vdim_X_Xdim = zeros(Vdim,Xdim);
0105 Zeros_Xdim_X_Ndim = zeros(Xdim,Ndim);
0106 Zeros_Ndim_X_Xdim = zeros(Ndim,Xdim);
0107 
0108 <span class="comment">% Get index vectors for any of the state or observation vector components that are angular quantities</span>
0109 <span class="comment">% which have discontinuities at +- Pi radians ?</span>
0110 
0111 sA_IdxVec = InferenceDS.stateAngleCompIdxVec;
0112 oA_IdxVec = InferenceDS.obsAngleCompIdxVec;
0113 
0114 
0115 nsp1                    = 2*(Xdim+Vdim) + 1;             <span class="comment">% number of sigma points (first set)</span>
0116 nsp2                    = 2*(Xdim+Ndim) + 1;             <span class="comment">% number of sigma points (second set)</span>
0117 
0118 <span class="keyword">if</span> (U1dim==0), UU1 = zeros(0,nsp1); <span class="keyword">end</span>
0119 <span class="keyword">if</span> (U2dim==0), UU2 = zeros(0,nsp2); <span class="keyword">end</span>
0120 
0121 Sv = chol(pNoise.cov)';                     <span class="comment">% matrix square root of process noise covariance</span>
0122 Sn = chol(oNoise.cov)';                     <span class="comment">% matrix square root of measurement noise covariance</span>
0123 Sx = chol(Pstate)';                                     <span class="comment">% matrix square root of state covariance</span>
0124 
0125 <span class="comment">%--------------------------------------- Loop over all input vectors --------------------------------------------</span>
0126 <span class="keyword">for</span> i=1:NOV,
0127 
0128     <span class="keyword">if</span> U1dim, UU1 = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(U1(:,i),nsp1); <span class="keyword">end</span>
0129     <span class="keyword">if</span> U2dim, UU2 = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(U2(:,i),nsp2); <span class="keyword">end</span>
0130 
0131     <span class="comment">%------------------------------------------------------</span>
0132     <span class="comment">% TIME UPDATE</span>
0133 
0134     Z   = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>([state; pNoise.mu],nsp1);
0135     Zm  = Z;                                                 <span class="comment">% copy needed for possible angle components section</span>
0136     Sz  = [Sx Zeros_Xdim_X_Vdim; Zeros_Vdim_X_Xdim Sv];
0137     hSz = h*Sz;
0138     hSzM = [hSz -hSz];
0139     Z(:,2:nsp1) = Z(:,2:nsp1) + hSzM;
0140 
0141     <span class="comment">%-- Calculate predicted state mean, dealing with angular discontinuities if needed</span>
0142     <span class="keyword">if</span> isempty(sA_IdxVec)
0143         X_ = InferenceDS.ffun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Vdim,:), UU1);  <span class="comment">% propagate sigma-points through process model</span>
0144         xh_(:,i) = W1(1,1)*X_(:,1) + W1(1,2)*sum(X_(:,2:nsp1),2);
0145         A = W1(2,1) * ( X_(:,2:Xdim+Vdim+1) - X_(:,Xdim+Vdim+2:nsp1) ) ;
0146         B = W1(2,2) * ( X_(:,2:Xdim+Vdim+1) + X_(:,Xdim+Vdim+2:nsp1) - <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(2*X_(:,1),Xdim+Vdim));
0147     <span class="keyword">else</span>
0148         Z(sA_IdxVec,2:nsp1) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(Zm(sA_IdxVec,2:nsp1), hSzM(sA_IdxVec,:));      <span class="comment">% fix sigma-point set for angular components</span>
0149         X_ = InferenceDS.ffun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Vdim,:), UU1); <span class="comment">% propagate sigma-points through process model</span>
0150         state_pivotA = X_(sA_IdxVec,1);                                <span class="comment">% extract pivot angle</span>
0151         X_(sA_IdxVec,1) = 0;
0152         X_(sA_IdxVec,2:end) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(X_(sA_IdxVec,2:end),<a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(state_pivotA,nsp1-1));  <span class="comment">% subtract pivot angle mod 2pi</span>
0153         xh_(:,i) = W1(1,1)*X_(:,1) + W1(1,2)*sum(X_(:,2:nsp1),2);
0154         xh_(sA_IdxVec,i) = 0;
0155         <span class="keyword">for</span> k=2:nsp1,
0156             xh_(sA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(xh_(sA_IdxVec,i), W1(1,2)*X_(sA_IdxVec,k));     <span class="comment">% calculate CDKF mean ... mod 2pi</span>
0157         <span class="keyword">end</span>
0158         A = W1(2,1) * ( X_(:,2:Xdim+Vdim+1) - X_(:,Xdim+Vdim+2:nsp1) ) ;
0159         B = W1(2,2) * ( X_(:,2:Xdim+Vdim+1) + X_(:,Xdim+Vdim+2:nsp1) - <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(2*X_(:,1),Xdim+Vdim));
0160         A(sA_IdxVec,:) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(W1(2,1)*X_(sA_IdxVec,2:Xdim+Vdim+1), W1(2,1)*X_(sA_IdxVec,Xdim+Vdim+2:nsp1));
0161         B(sA_IdxVec,:) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(W1(2,2)*X_(sA_IdxVec,2:Xdim+Vdim+1), W1(2,2)*X_(sA_IdxVec,Xdim+Vdim+2:nsp1));
0162         <span class="comment">% Note for above line : Remember, X_(sA_IdxVec,1) = 0, so the last term of B expression need not be subtracted</span>
0163         xh_(sA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(xh_(sA_IdxVec,i), state_pivotA);  <span class="comment">% add pivot angle back to calculate actual predicted mean</span>
0164     <span class="keyword">end</span>
0165 
0166     [temp,Sx_] = qr([A B]',0);
0167     Sx_= Sx_';
0168 
0169     Z = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>([xh_(:,i); oNoise.mu],nsp2);
0170     Zm = Z;                                             <span class="comment">% copy needed for possible angle components section</span>
0171     Sz = [Sx_ Zeros_Xdim_X_Ndim; Zeros_Ndim_X_Xdim Sn];
0172     hSz = h*Sz;
0173     hSzM = [hSz -hSz];
0174     Z(:,2:nsp2) = Z(:,2:nsp2) + hSzM;
0175 
0176     <span class="comment">%-- Calculate predicted observation mean, dealing with angular discontinuities if needed</span>
0177     <span class="keyword">if</span> isempty(oA_IdxVec)
0178         Y_ = InferenceDS.hfun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Ndim,:), UU2);
0179         yh_(:,i) = W2(1,1)*Y_(:,1) + W2(1,2)*sum(Y_(:,2:nsp2),2);
0180         C = W2(2,1) * ( Y_(:,2:Xdim+Ndim+1) - Y_(:,Xdim+Ndim+2:nsp2) );
0181         D = W2(2,2) * ( Y_(:,2:Xdim+Ndim+1) + Y_(:,Xdim+Ndim+2:nsp2) - <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(2*Y_(:,1),Xdim+Ndim));
0182     <span class="keyword">else</span>
0183         Z(oA_IdxVec,2:nsp2) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(Zm(oA_IdxVec,2:nsp2), hSzM(oA_IdxVec,:));  <span class="comment">% fix sigma-point set for angular components</span>
0184         Y_ = InferenceDS.hfun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Ndim,:), UU2);
0185         obs_pivotA = Y_(oA_IdxVec,1);      <span class="comment">% extract pivot angle</span>
0186         Y_(oA_IdxVec,1) = 0;
0187         Y_(oA_IdxVec,2:nsp2) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(Y_(oA_IdxVec,2:nsp2),<a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(obs_pivotA,nsp2-1));  <span class="comment">% subtract pivot angle mod 2pi</span>
0188         yh_(:,i) = W2(1,1)*Y_(:,1) + W2(1,2)*sum(Y_(:,2:nsp2),2);           <span class="comment">% pediction of observation</span>
0189         yh_(oA_IdxVec,i) = 0;
0190         <span class="keyword">for</span> k=2:nsp2,
0191            yh_(oA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(yh_(oA_IdxVec,i), W2(1,2)*Y_(oA_IdxVec,k));   <span class="comment">% calculate CDKF mean ... mod 2pi</span>
0192         <span class="keyword">end</span>
0193         C = W2(2,1) * ( Y_(:,2:Xdim+Ndim+1) - Y_(:,Xdim+Ndim+2:nsp2) );
0194         D = W2(2,2) * ( Y_(:,2:Xdim+Ndim+1) + Y_(:,Xdim+Ndim+2:nsp2) - <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(2*Y_(:,1),Xdim+Ndim));
0195         C(oA_IdxVec,:) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(W2(2,1)*Y_(oA_IdxVec,2:Xdim+Ndim+1), W2(2,1)*Y_(oA_IdxVec, Xdim+Ndim+2:nsp2));
0196         D(oA_IdxVec,:) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(W2(2,2)*Y_(oA_IdxVec,2:Xdim+Ndim+1), W2(2,2)*Y_(oA_IdxVec, Xdim+Ndim+2:nsp2));
0197         <span class="comment">% Note for above line : Remember, Y_(oA_IdxVec,1) = 0, so the last term of D expression need not be subtracted</span>
0198         yh_(oA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(yh_(oA_IdxVec,i), obs_pivotA);  <span class="comment">% add pivot angle back to calculate actual predicted mean</span>
0199     <span class="keyword">end</span>
0200 
0201     [temp,Sy] = qr([C D]',0);
0202     Sy = Sy';
0203 
0204 
0205     <span class="comment">%------------------------------------------------------</span>
0206     <span class="comment">% MEASUREMENT UPDATE</span>
0207 
0208     Syx1 = C(:,1:Xdim);
0209     Syw1 = C(:,Xdim+1:end);
0210 
0211     Pxy = Sx_*Syx1';
0212 
0213     KG = (Pxy / Sy') / Sy;
0214 
0215     <span class="keyword">if</span> isempty(InferenceDS.innovation)
0216         inov(:,i) = obs(:,i) - yh_(:,i);
0217         <span class="keyword">if</span> ~isempty(oA_IdxVec)
0218           inov(oA_IdxVec,i) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(obs(oA_IdxVec,i), yh_(oA_IdxVec,i));
0219         <span class="keyword">end</span>
0220     <span class="keyword">else</span>
0221         inov(:,i) = InferenceDS.innovation( InferenceDS, obs(:,i), yh_(:,i));  <span class="comment">% inovation (observation error)</span>
0222     <span class="keyword">end</span>
0223 
0224 
0225     <span class="keyword">if</span> isempty(sA_IdxVec)
0226        xh(:,i) = xh_(:,i) + KG*inov(:,i);
0227     <span class="keyword">else</span>
0228        upd = KG*inov(:,i);
0229        xh(:,i) = xh_(:,i) + upd;
0230        xh(sA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(xh_(sA_IdxVec,i), upd(sA_IdxVec));
0231     <span class="keyword">end</span>
0232 
0233     state = xh(:,i);
0234 
0235     [temp,Sx] = qr([Sx_-KG*Syx1 KG*Syw1 KG*D]',0);
0236     Sx=Sx';
0237 
0238     Px = Sx*Sx';
0239 
0240 
0241     <span class="keyword">if</span> pNoise.adaptMethod <span class="keyword">switch</span> InferenceDS.inftype
0242     <span class="comment">%---------------------- UPDATE PROCESS NOISE SOURCE IF NEEDED --------------------------------------------</span>
0243     <span class="keyword">case</span> <span class="string">'parameter'</span>  <span class="comment">%--- parameter estimation</span>
0244         <span class="keyword">switch</span> pNoise.adaptMethod
0245         <span class="keyword">case</span> <span class="string">'anneal'</span>
0246             pNoise.cov = diag(max(pNoise.adaptParams(1) * diag(pNoise.cov) , pNoise.adaptParams(2)));
0247         <span class="keyword">case</span> <span class="string">'lambda-decay'</span>
0248             pNoise.cov = (1/pNoise.adaptParams(1)-1) * Px;
0249         <span class="keyword">case</span> <span class="string">'robbins-monro'</span>
0250             nu = 1/pNoise.adaptParams(1);
0251             pNoise.cov = (1-nu)*pNoise.cov + nu*KG*(KG*inov*inov')';
0252             pNoise.adaptParams(1) = min(pNoise.adaptParams(1)+1, pNoise.adaptParams(2));
0253         <span class="keyword">otherwise</span>
0254             error(<span class="string">' [ cdkf ] Unknown process noise adaptation method!'</span>);
0255         <span class="keyword">end</span>
0256 
0257         Sv = chol(pNoise.cov)';
0258 
0259     <span class="keyword">case</span> <span class="string">'state'</span>  <span class="comment">%--- state estimation</span>
0260         <span class="keyword">switch</span> pNoise.adaptMethod
0261         <span class="keyword">case</span> <span class="string">'robbins-monro'</span>
0262             nu = 1/pNoise.adaptParams(1);
0263             pNoise.cov = (1-nu)*pNoise.cov + nu*KG*(KG*inov*inov')';
0264             pNoise.adaptParams(1) = min(pNoise.adaptParams(1)+1, pNoise.adaptParams(2));
0265         <span class="keyword">otherwise</span>
0266             error(<span class="string">' [ cdkf ] Process noise adaptation method not allowed!'</span>);
0267         <span class="keyword">end</span>
0268 
0269         Sv = chol(pNoise.cov)';
0270 
0271     <span class="keyword">case</span> <span class="string">'joint'</span>  <span class="comment">%--- joint estimation</span>
0272         idx = pNoise.idxArr(<span class="keyword">end</span>,:); <span class="comment">% get indexs of parameter block of combo-gaussian noise source</span>
0273         ind1 = idx(1); ind2 = idx(2);
0274         <span class="keyword">switch</span> pNoise.adaptMethod
0275         <span class="keyword">case</span> <span class="string">'anneal'</span>
0276             pNoise.cov(ind1:ind2,ind1:ind2) = diag(max(pNoise.adaptParams(1) * diag(pNoise.cov(ind1:ind2,ind1:ind2)), pNoise.adaptParams(2)));
0277         <span class="keyword">case</span> <span class="string">'lambda-decay'</span>
0278             param_length = ind2-ind1+1;
0279             pNoise.cov(ind1:ind2,ind1:ind2) = (1/pNoise.adaptParams(1)-1) * Px(end-param_length+1:<span class="keyword">end</span>,end-param_length+1:end);
0280         <span class="keyword">case</span> <span class="string">'robbins-monro'</span>
0281             param_length = ind2-ind1+1;
0282             nu = 1/pNoise.adaptParams(1);
0283             subKG = KG(end-param_length+1:<span class="keyword">end</span>,:);
0284             pNoise.cov(ind1:ind2,ind1:ind2) = (1-nu)*pNoise.cov(ind1:ind2,ind1:ind2) + nu*subKG*(subKG*inov*inov')';
0285             pNoise.adaptParams(1) = min(pNoise.adaptParams(1)+1, pNoise.adaptParams(2));
0286         <span class="keyword">otherwise</span>
0287             error(<span class="string">' [ cdkf ] Unknown process noise adaptation method!'</span>);
0288         <span class="keyword">end</span>
0289 
0290         Sv = chol(pNoise.cov)';
0291 
0292     <span class="comment">%--------------------------------------------------------------------------------------------------</span>
0293     <span class="keyword">end</span>; 
0294     <span class="keyword">end</span>
0295 
0296 <span class="keyword">end</span>   <span class="comment">%--- for loop</span>
0297 
0298 
0299 <span class="keyword">if</span> (nargout&gt;4),
0300   InternalVariablesDS.xh_   = xh_;
0301   InternalVariablesDS.Px_   = Sx_*Sx_';
0302   InternalVariablesDS.yh_   = yh_;
0303   InternalVariablesDS.inov  = inov;
0304   InternalVariablesDS.Pinov = Sy*Sy';
0305   InternalVariablesDS.KG    = KG;
0306 <span class="keyword">end</span>
0307</pre></div>
<hr><address>Generated on Tue 26-Sep-2006 10:36:21 by <strong><a href="http://www.artefact.tk/software/matlab/m2html/">m2html</a></strong> &copy; 2003</address>
</body>
</html>